// our point structure to make things nice.
struct LongPoint {
	long x;
	long y;
 	long z;
        long f;
};

struct FloatPoint {
	float x;
	float y;
 	float z;
        float f;
};

FloatPoint current_units;
FloatPoint target_units;
FloatPoint delta_units;

FloatPoint current_steps;
FloatPoint target_steps;
FloatPoint delta_steps;

boolean abs_mode = true;   //0 = incremental; 1 = absolute

//default to mm for units
float x_units = X_STEPS_PER_MM;
float y_units = Y_STEPS_PER_MM;
float z_units = Z_STEPS_PER_MM;
float f_units = F_STEPS_PER_MM;
float curve_section = CURVE_SECTION_INCHES;

//our direction vars
byte x_direction = 1;
byte y_direction = 1;
byte z_direction = 1;
byte f_direction = 1;

float feedrate_adjust = 100.0;

//init our string processing
void init_process_string()
{
	//init our command
	for (byte i=0; i<COMMAND_SIZE; i++)
		word_old[i] = 0;
	serial_count = 0;
}

//our feedrate variables.
float feedrate = 0.0;
long feedrate_micros = 0;

//Read the string and execute instructions
void process_string(char instruction[], int size)
{
	//the character / means delete block... used for comments and stuff.
	if (instruction[0] == '/')
	{
		//Serial.print("ok");
                //Serial.print(byte(78));
		return;
	}

	//init baby!
	FloatPoint fp;
	fp.x = 0.0;
	fp.y = 0.0;
	fp.z = 0.0;
        fp.f = 0.0;

	byte code = 0;;
		
	//did we get a gcode?
	if (
		has_command('G', instruction, size) ||
		has_command('X', instruction, size) ||
		has_command('Y', instruction, size) ||
		has_command('Z', instruction, size) ||
                has_command('F', instruction, size)
	)
	{
		//which one?
		code = (int)search_string('G', instruction, size);
		
		// Get co-ordinates if required by the code type given
		switch (code)
		{
			case 0:
			case 1:
			case 2:
			case 3:
				if(abs_mode)
				{
					//we do it like this to save time. makes curves better.
					//eg. if only x and y are specified, we dont have to waste time looking up z.
					if (has_command('X', instruction, size))
						fp.x = search_string('X', instruction, size);
					else
						fp.x = current_units.x;
				
					if (has_command('Y', instruction, size))
						fp.y = search_string('Y', instruction, size);
					else
						fp.y = current_units.y;
				
					if (has_command('Z', instruction, size))
						fp.z = search_string('Z', instruction, size);
					else
						fp.z = current_units.z;

                                        if (has_command('F', instruction, size))
						fp.f = search_string('F', instruction, size);
					else
						fp.f = current_units.f;
				}
				else
				{
					fp.x = search_string('X', instruction, size) + current_units.x;
					fp.y = search_string('Y', instruction, size) + current_units.y;
					fp.z = search_string('Z', instruction, size) + current_units.z;
                                        fp.f = search_string('F', instruction, size) + current_units.f;
				}
			break;
		}

		//do something!
		switch (code)
		{
			//Rapid Positioning / Linear Interpolation
			//these are basically the same thing.
			case 0:
			case 1:
				//set our target.
				set_target(fp.x, fp.y, fp.z, fp.f);
                                //Serial.println(fp.x);

				//do we have a set speed?
				if (has_command('G', instruction, size))
				{
					//adjust if we have a specific feedrate.
					if (code == 1)
					{
						//how fast do we move?
						feedrate = search_string('H', instruction, size);
						if (feedrate > 0)
							feedrate_micros = calculate_feedrate_delay(feedrate);
						//nope, no feedrate
						else
							feedrate_micros = getMaxSpeed();
					}
					//use our max for normal moves.
					else
						feedrate_micros = getMaxSpeed();
				}
				//nope, just coordinates!
				else
				{
					//do we have a feedrate yet?
					if (feedrate > 0)
						feedrate_micros = calculate_feedrate_delay(feedrate);
					//nope, no feedrate
					else
						feedrate_micros = getMaxSpeed();
				}

				//finally move.
				dda_move(feedrate_micros);
			break;
			
			//Clockwise arc...not used
			case 2:

			//Counterclockwise arc...not used
			case 3:
				
			//Dwell
			case 4:
				delay((int)search_string('P', instruction, size));
			break;

			//Inches for Units
			case 20:
				x_units = X_STEPS_PER_INCH;
				y_units = Y_STEPS_PER_INCH;
				z_units = Z_STEPS_PER_INCH;
                                f_units = F_STEPS_PER_INCH;
				curve_section = CURVE_SECTION_INCHES;
				
				calculate_deltas();
			break;

                        //mm for Units
			case 21:
				x_units = X_STEPS_PER_MM;
				y_units = Y_STEPS_PER_MM;
				z_units = Z_STEPS_PER_MM;
                                f_units = F_STEPS_PER_MM;
				curve_section = CURVE_SECTION_MM;
				
				calculate_deltas();
			break;

                        //adjust feedrate
                        case 24:
                                feedrate_adjust = search_string('S', instruction, size);
                                break;

			//go home.
			case 28:
				set_target(0.0, 0.0, 0.0, 0.0);
				dda_move(getMaxSpeed());
			break;

			//go home via an intermediate point.
			case 30:
				fp.x = search_string('X', instruction, size);
				fp.y = search_string('Y', instruction, size);
				fp.z = search_string('Z', instruction, size);
                                fp.f = search_string('F', instruction, size);
                                
				//set our target.
				if(abs_mode)
				{
					if (!has_command('X', instruction, size))
						fp.x = current_units.x;
					if (!has_command('Y', instruction, size))
						fp.y = current_units.y;
					if (!has_command('Z', instruction, size))
						fp.z = current_units.z;
                                        if (!has_command('F', instruction, size))
						fp.f = current_units.f;
						
					set_target(fp.x, fp.y, fp.z, fp.f);
				}
				else
					set_target(current_units.x + fp.x, current_units.y + fp.y, current_units.z + fp.z, current_units.f + fp.f);
				
				//go there.
				dda_move(getMaxSpeed());

				//go home.
				set_target(0.0, 0.0, 0.0, 0.0);
				dda_move(getMaxSpeed());
			break;

			//Absolute Positioning
			case 90:
				abs_mode = true;
			break;

			//Incremental Positioning
			case 91:
				abs_mode = false;
			break;

			//Set as home
			case 92:
				set_position(0.0, 0.0, 0.0, 0.0);
			break;
                        
			default:
				Serial.print("huh? G");
				//Serial.println(code,DEC);
		}
	}
	
	//find us an m code.
	if (has_command('M', instruction, size))
	{
		code = search_string('M', instruction, size);
		switch (code)
		{
			//TODO: this is a bug because search_string returns 0.  gotta fix that.
			case 0:
				true;
			break;
	                
                        // fire camera relay
			case 100:
                            camera_shutter();
			break;

                           // wakeflashes 1 and 2
			case 111:
                            wakeflash();
			break;

                        case 114:
                                Serial.print("ok X:"); Serial.print(current_steps.x);
                                Serial.print(" Y:"); Serial.print(current_steps.y);
                                Serial.print(" Z:"); Serial.println(current_steps.z);
                                return;
                        break;
                        
			default:
				Serial.print("Huh? M");
				//Serial.println(code);
		}		
	}
	
	//tell our host we're done.
        Serial.println("R"); // send a "READY" command to let us know that we are ready for the next command

}

//look for the number that appears after the char key and return it
double search_string(char key, char instruction[], int string_size)
{
	char temp[10] = "";

	for (byte i=0; i<string_size; i++)
	{
		if (instruction[i] == key)
		{
			i++;      
			int k = 0;
			while (i < string_size && k < 10)
			{
				if (instruction[i] == 0 || instruction[i] == ' ')
					break;

				temp[k] = instruction[i];
				i++;
				k++;
			}
			return strtod(temp, NULL);
		}
	}
	
	return 0;
}

//look for the command if it exists.
bool has_command(char key, char instruction[], int string_size)
{
	for (byte i=0; i<string_size; i++)
	{
		if (instruction[i] == key)
			return true;
	}
	
	return false;
}
